home *** CD-ROM | disk | FTP | other *** search
/ AOL File Library: 2,801 to 2,900 / aol-file-protocol-4400-2801-to-2900.zip / AOLDLs / C++ Files Library / Graphic Gems I, II & III (C_C++) / Graphics Gems C Code.sea / GemsIII / GraphicsGems.c < prev    next >
Text File  |  1992-06-16  |  13KB  |  471 lines
  1. /* GGVecLib.c */
  2. /* 
  3. 2d and 3d Vector C Library 
  4. by Andrew Glassner
  5. from "Graphics Gems", Academic Press, 1990
  6. */
  7.  
  8. #include <math.h>
  9. #include "GraphicsGems.h"
  10.  
  11. /******************/
  12. /*   2d Library   */
  13. /******************/
  14.  
  15. /* returns squared length of input vector */    
  16. double V2SquaredLength(a) 
  17. Vector2 *a;
  18. {       return((a->x * a->x)+(a->y * a->y));
  19.         };
  20.         
  21. /* returns length of input vector */
  22. double V2Length(a) 
  23. Vector2 *a;
  24. {
  25.         return(sqrt(V2SquaredLength(a)));
  26.         };
  27.         
  28. /* negates the input vector and returns it */
  29. Vector2 *V2Negate(v) 
  30. Vector2 *v;
  31. {
  32.         v->x = -v->x;  v->y = -v->y;
  33.         return(v);
  34.         };
  35.  
  36. /* normalizes the input vector and returns it */
  37. Vector2 *V2Normalize(v) 
  38. Vector2 *v;
  39. {
  40. double len = V2Length(v);
  41.         if (len != 0.0) { v->x /= len;  v->y /= len; };
  42.         return(v);
  43.         };
  44.  
  45.  
  46. /* scales the input vector to the new length and returns it */
  47. Vector2 *V2Scale(v, newlen) 
  48. Vector2 *v;
  49. double newlen;
  50. {
  51. double len = V2Length(v);
  52.         if (len != 0.0) { v->x *= newlen/len;   v->y *= newlen/len; };
  53.         return(v);
  54.         };
  55.  
  56. /* return vector sum c = a+b */
  57. Vector2 *V2Add(a, b, c)
  58. Vector2 *a, *b, *c;
  59. {
  60.         c->x = a->x+b->x;  c->y = a->y+b->y;
  61.         return(c);
  62.         };
  63.         
  64. /* return vector difference c = a-b */
  65. Vector2 *V2Sub(a, b, c)
  66. Vector2 *a, *b, *c;
  67. {
  68.         c->x = a->x-b->x;  c->y = a->y-b->y;
  69.         return(c);
  70.         };
  71.  
  72. /* return the dot product of vectors a and b */
  73. double V2Dot(a, b) 
  74. Vector2 *a, *b; 
  75. {
  76.         return((a->x*b->x)+(a->y*b->y));
  77.         };
  78.  
  79. /* linearly interpolate between vectors by an amount alpha */
  80. /* and return the resulting vector. */
  81. /* When alpha=0, result=lo.  When alpha=1, result=hi. */
  82. Vector2 *V2Lerp(lo, hi, alpha, result) 
  83. Vector2 *lo, *hi, *result; 
  84. double alpha;
  85. {
  86.         result->x = LERP(alpha, lo->x, hi->x);
  87.         result->y = LERP(alpha, lo->y, hi->y);
  88.         return(result);
  89.         };
  90.  
  91.  
  92. /* make a linear combination of two vectors and return the result. */
  93. /* result = (a * ascl) + (b * bscl) */
  94. Vector2 *V2Combine (a, b, result, ascl, bscl) 
  95. Vector2 *a, *b, *result;
  96. double ascl, bscl;
  97. {
  98.         result->x = (ascl * a->x) + (bscl * b->x);
  99.         result->y = (ascl * a->y) + (bscl * b->y);
  100.         return(result);
  101.         };
  102.  
  103. /* multiply two vectors together component-wise */
  104. Vector2 *V2Mul (a, b, result) 
  105. Vector2 *a, *b, *result;
  106. {
  107.         result->x = a->x * b->x;
  108.         result->y = a->y * b->y;
  109.         return(result);
  110.         };
  111.  
  112. /* return the distance between two points */
  113. double V2DistanceBetween2Points(a, b)
  114. Point2 *a, *b;
  115. {
  116. double dx = a->x - b->x;
  117. double dy = a->y - b->y;
  118.         return(sqrt((dx*dx)+(dy*dy)));
  119.         };
  120.  
  121. /* return the vector perpendicular to the input vector a */
  122. Vector2 *V2MakePerpendicular(a, ap)
  123. Vector2 *a, *ap;
  124. {
  125.         ap->x = -a->y;
  126.         ap->y = a->x;
  127.         return(ap);
  128.         };
  129.  
  130. /* create, initialize, and return a new vector */
  131. Vector2 *V2New(x, y)
  132. double x, y;
  133. {
  134. Vector2 *v = NEWTYPE(Vector2);
  135.         v->x = x;  v->y = y; 
  136.         return(v);
  137.         };
  138.         
  139.  
  140. /* create, initialize, and return a duplicate vector */
  141. Vector2 *V2Duplicate(a)
  142. Vector2 *a;
  143. {
  144. Vector2 *v = NEWTYPE(Vector2);
  145.         v->x = a->x;  v->y = a->y; 
  146.         return(v);
  147.         };
  148.         
  149. /* multiply a point by a projective matrix and return the transformed point */
  150. Point2 *V2MulPointByProjMatrix(pin, m, pout)
  151. Point2 *pin, *pout;
  152. Matrix3 *m;
  153. {
  154. double w;
  155.         pout->x = (pin->x * m->element[0][0]) + 
  156.              (pin->y * m->element[1][0]) + m->element[2][0];
  157.         pout->y = (pin->x * m->element[0][1]) + 
  158.              (pin->y * m->element[1][1]) + m->element[2][1];
  159.         w    = (pin->x * m->element[0][2]) + 
  160.              (pin->y * m->element[1][2]) + m->element[2][2];
  161.         if (w != 0.0) { pout->x /= w;  pout->y /= w; }
  162.         return(pout);
  163.         };
  164.  
  165. /* multiply together matrices c = ab */
  166. /* note that c must not point to either of the input matrices */
  167. Matrix3 *V2MatMul(a, b, c)
  168. Matrix3 *a, *b, *c;
  169. {
  170. int i, j, k;
  171.         for (i=0; i<3; i++) {
  172.                 for (j=0; j<3; j++) {
  173.                         c->element[i][j] = 0;
  174.                 for (k=0; k<3; k++) c->element[i][j] += 
  175.                                 a->element[i][k] * b->element[k][j];
  176.                         };
  177.                 };
  178.         return(c);
  179.         };
  180.  
  181. /* transpose matrix a, return b */
  182. Matrix3 *TransposeMatrix3(a, b)
  183. Matrix3 *a, *b;
  184. {
  185. int i, j;
  186.         for (i=0; i<3; i++) {
  187.                 for (j=0; j<3; j++)
  188.                         b->element[i][j] = a->element[j][i];
  189.                 };
  190.         return(b);
  191.         };
  192.  
  193.  
  194.  
  195.  
  196. /******************/
  197. /*   3d Library   */
  198. /******************/
  199.         
  200. /* returns squared length of input vector */    
  201. double V3SquaredLength(a) 
  202. Vector3 *a;
  203. {
  204.         return((a->x * a->x)+(a->y * a->y)+(a->z * a->z));
  205.         };
  206.  
  207. /* returns length of input vector */
  208. double V3Length(a) 
  209. Vector3 *a;
  210. {
  211.         return(sqrt(V3SquaredLength(a)));
  212.         };
  213.  
  214. /* negates the input vector and returns it */
  215. Vector3 *V3Negate(v) 
  216. Vector3 *v;
  217. {
  218.         v->x = -v->x;  v->y = -v->y;  v->z = -v->z;
  219.         return(v);
  220.         };
  221.  
  222. /* normalizes the input vector and returns it */
  223. Vector3 *V3Normalize(v) 
  224. Vector3 *v;
  225. {
  226. double len = V3Length(v);
  227.         if (len != 0.0) { v->x /= len;  v->y /= len; v->z /= len; };
  228.         return(v);
  229.         };
  230.  
  231. /* scales the input vector to the new length and returns it */
  232. Vector3 *V3Scale(v, newlen) 
  233. Vector3 *v;
  234. double newlen;
  235. {
  236. double len = V3Length(v);
  237.         if (len != 0.0) {
  238.         v->x *= newlen/len;   v->y *= newlen/len;  v->z *= newlen/len;
  239.         };
  240.         return(v);
  241.         };
  242.  
  243.  
  244. /* return vector sum c = a+b */
  245. Vector3 *V3Add(a, b, c)
  246. Vector3 *a, *b, *c;
  247. {
  248.         c->x = a->x+b->x;  c->y = a->y+b->y;  c->z = a->z+b->z;
  249.         return(c);
  250.         };
  251.         
  252. /* return vector difference c = a-b */
  253. Vector3 *V3Sub(a, b, c)
  254. Vector3 *a, *b, *c;
  255. {
  256.         c->x = a->x-b->x;  c->y = a->y-b->y;  c->z = a->z-b->z;
  257.         return(c);
  258.         };
  259.  
  260. /* return the dot product of vectors a and b */
  261. double V3Dot(a, b) 
  262. Vector3 *a, *b; 
  263. {
  264.         return((a->x*b->x)+(a->y*b->y)+(a->z*b->z));
  265.         };
  266.  
  267. /* linearly interpolate between vectors by an amount alpha */
  268. /* and return the resulting vector. */
  269. /* When alpha=0, result=lo.  When alpha=1, result=hi. */
  270. Vector3 *V3Lerp(lo, hi, alpha, result) 
  271. Vector3 *lo, *hi, *result; 
  272. double alpha;
  273. {
  274.         result->x = LERP(alpha, lo->x, hi->x);
  275.         result->y = LERP(alpha, lo->y, hi->y);
  276.         result->z = LERP(alpha, lo->z, hi->z);
  277.         return(result);
  278.         };
  279.  
  280. /* make a linear combination of two vectors and return the result. */
  281. /* result = (a * ascl) + (b * bscl) */
  282. Vector3 *V3Combine (a, b, result, ascl, bscl) 
  283. Vector3 *a, *b, *result;
  284. double ascl, bscl;
  285. {
  286.         result->x = (ascl * a->x) + (bscl * b->x);
  287.         result->y = (ascl * a->y) + (bscl * b->y);
  288.         result->y = (ascl * a->z) + (bscl * b->z);
  289.         return(result);
  290.         };
  291.  
  292.  
  293. /* multiply two vectors together component-wise and return the result */
  294. Vector3 *V3Mul (a, b, result) 
  295. Vector3 *a, *b, *result;
  296. {
  297.         result->x = a->x * b->x;
  298.         result->y = a->y * b->y;
  299.         result->z = a->z * b->z;
  300.         return(result);
  301.         };
  302.  
  303. /* return the distance between two points */
  304. double V3DistanceBetween2Points(a, b)
  305. Point3 *a, *b;
  306. {
  307. double dx = a->x - b->x;
  308. double dy = a->y - b->y;
  309. double dz = a->z - b->z;
  310.         return(sqrt((dx*dx)+(dy*dy)+(dz*dz)));
  311.         };
  312.  
  313. /* return the cross product c = a cross b */
  314. Vector3 *V3Cross(a, b, c)
  315. Vector3 *a, *b, *c;
  316. {
  317.         c->x = (a->y*b->z) - (a->z*b->y);
  318.         c->y = (a->z*b->x) - (a->x*b->z);
  319.         c->z = (a->x*b->y) - (a->y*b->x);
  320.         return(c);
  321.         };
  322.  
  323. /* create, initialize, and return a new vector */
  324. Vector3 *V3New(x, y, z)
  325. double x, y, z;
  326. {
  327. Vector3 *v = NEWTYPE(Vector3);
  328.         v->x = x;  v->y = y;  v->z = z;
  329.         return(v);
  330.         };
  331.  
  332. /* create, initialize, and return a duplicate vector */
  333. Vector3 *V3Duplicate(a)
  334. Vector3 *a;
  335. {
  336. Vector3 *v = NEWTYPE(Vector3);
  337.         v->x = a->x;  v->y = a->y;  v->z = a->z;
  338.         return(v);
  339.         };
  340.  
  341.         
  342. /* multiply a point by a matrix and return the transformed point */
  343. Point3 *V3MulPointByMatrix(pin, m, pout)
  344. Point3 *pin, *pout;
  345. Matrix3 *m;
  346. {
  347.         pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) + 
  348.                  (pin->z * m->element[2][0]);
  349.         pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) + 
  350.                  (pin->z * m->element[2][1]);
  351.         pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) + 
  352.                  (pin->z * m->element[2][2]);
  353.         return(pout);
  354.         };
  355.  
  356. /* multiply a point by a projective matrix and return the transformed point */
  357. Point3 *V3MulPointByProjMatrix(pin, m, pout)
  358. Point3 *pin, *pout;
  359. Matrix4 *m;
  360. {
  361. double w;
  362.         pout->x = (pin->x * m->element[0][0]) + (pin->y * m->element[1][0]) + 
  363.                  (pin->z * m->element[2][0]) + m->element[3][0];
  364.         pout->y = (pin->x * m->element[0][1]) + (pin->y * m->element[1][1]) + 
  365.                  (pin->z * m->element[2][1]) + m->element[3][1];
  366.         pout->z = (pin->x * m->element[0][2]) + (pin->y * m->element[1][2]) + 
  367.                  (pin->z * m->element[2][2]) + m->element[3][2];
  368.         w =    (pin->x * m->element[0][3]) + (pin->y * m->element[1][3]) + 
  369.                  (pin->z * m->element[2][3]) + m->element[3][3];
  370.         if (w != 0.0) { pout->x /= w;  pout->y /= w;  pout->z /= w; }
  371.         return(pout);
  372.         };
  373.  
  374. /* multiply together matrices c = ab */
  375. /* note that c must not point to either of the input matrices */
  376. Matrix4 *V3MatMul(a, b, c)
  377. Matrix4 *a, *b, *c;
  378. {
  379. int i, j, k;
  380.         for (i=0; i<4; i++) {
  381.                 for (j=0; j<4; j++) {
  382.                         c->element[i][j] = 0;
  383.                         for (k=0; k<4; k++) c->element[i][j] += 
  384.                                 a->element[i][k] * b->element[k][j];
  385.                         };
  386.                 };
  387.         return(c);
  388.         };
  389.  
  390. /* binary greatest common divisor by Silver and Terzian.  See Knuth */
  391. /* both inputs must be >= 0 */
  392. gcd(u, v)
  393. int u, v;
  394. {
  395. int k, t, f;
  396.         if ((u<0) || (v<0)) return(1); /* error if u<0 or v<0 */
  397.         k = 0;  f = 1;
  398.         while ((0 == (u%2)) && (0 == (v%2))) {
  399.                 k++;  u>>=1;  v>>=1,  f*=2;
  400.                 };
  401.         if (u&01) { t = -v;  goto B4; } else { t = u; }
  402.         B3: if (t > 0) { t >>= 1; } else { t = -((-t) >> 1); };
  403.         B4: if (0 == (t%2)) goto B3;
  404.  
  405.         if (t > 0) u = t; else v = -t;
  406.         if (0 != (t = u - v)) goto B3;
  407.         return(u*f);
  408.         };      
  409.  
  410. /***********************/
  411. /*   Useful Routines   */
  412. /***********************/
  413.  
  414. /* return roots of ax^2+bx+c */
  415. /* stable algebra derived from Numerical Recipes by Press et al.*/
  416. int quadraticRoots(a, b, c, roots)
  417. double a, b, c, *roots;
  418. {
  419. double d, q;
  420. int count = 0;
  421.         d = (b*b)-(4*a*c);
  422.         if (d < 0.0) { *roots = *(roots+1) = 0.0;  return(0); };
  423.         q =  -0.5 * (b + (SGN(b)*sqrt(d)));
  424.         if (a != 0.0)  { *roots++ = q/a; count++; }
  425.         if (q != 0.0) { *roots++ = c/q; count++; }
  426.         return(count);
  427.         };
  428.  
  429.  
  430. /* generic 1d regula-falsi step.  f is function to evaluate */
  431. /* interval known to contain root is given in left, right */
  432. /* returns new estimate */
  433. double RegulaFalsi(f, left, right)
  434. double (*f)(), left, right;
  435. {
  436. double d = (*f)(right) - (*f)(left);
  437.         if (d != 0.0) return (right - (*f)(right)*(right-left)/d);
  438.         return((left+right)/2.0);
  439.         };
  440.  
  441. /* generic 1d Newton-Raphson step. f is function, df is derivative */
  442. /* x is current best guess for root location. Returns new estimate */
  443. double NewtonRaphson(f, df, x)
  444. double (*f)(), (*df)(), x;
  445. {
  446. double d = (*df)(x);
  447.         if (d != 0.0) return (x-((*f)(x)/d));
  448.         return(x-1.0);
  449.         };
  450.  
  451.  
  452. /* hybrid 1d Newton-Raphson/Regula Falsi root finder. */
  453. /* input function f and its derivative df, an interval */
  454. /* left, right known to contain the root, and an error tolerance */
  455. /* Based on Blinn */
  456. double findroot(left, right, tolerance, f, df)
  457. double left, right, tolerance;
  458. double (*f)(), (*df)();
  459. {
  460. double newx = left;
  461.         while (ABS((*f)(newx)) > tolerance) {
  462.                 newx = NewtonRaphson(f, df, newx);
  463.                 if (newx < left || newx > right) 
  464.                         newx = RegulaFalsi(f, left, right);
  465.                 if ((*f)(newx) * (*f)(left) <= 0.0) right = newx;  
  466.                         else left = newx;
  467.                 };
  468.         return(newx);
  469.         }; 
  470.  
  471.